import os
import shutil
import yaml
from launch import LaunchDescription
from launch.actions import (
    ExecuteProcess,
    DeclareLaunchArgument,
    OpaqueFunction,
)
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_prefix
package_name = 'semantic_nav'


def launch_setup(context, *args, **kwargs):
    # Set the name of the bag file
    bag_name = LaunchConfiguration('bag_name').perform(context)
    use_sim_time = LaunchConfiguration("use_sim_time").perform(context)
    package_path = get_package_prefix(package_name)
    bag_path = os.path.join(package_path, 'bag', bag_name)
    qos_profile = os.path.join(package_path, 'config', 'bag_record_qos_profile.yaml')
    # Check if the bag file exists and delete it if it does
    if os.path.exists(bag_path):
        shutil.rmtree(bag_path)
    # Set the topics to record
    with open(qos_profile, 'r') as file:
        yaml_data = yaml.safe_load(file)
    topics_to_record = list(yaml_data.keys())
    record_cmd = ['ros2', 'bag', 'record', '-o', bag_path, '--qos-profile-overrides-path', qos_profile]
    if use_sim_time:
        record_cmd.append('--use-sim-time')
    record_process = ExecuteProcess(
        cmd=record_cmd + topics_to_record,
        output='screen'
    )
    return [record_process]


def generate_launch_description():
    declare_bag_name_cmd = DeclareLaunchArgument(
        'bag_name',
        default_value='test_bag',
        description='The bag file name',
    )
    declare_use_sim_time_cmd = DeclareLaunchArgument(
        'use_sim_time',
        default_value='False',
        description='Use simulation (Gazebo) clock if true',
    )

    return LaunchDescription([
        declare_bag_name_cmd,
        declare_use_sim_time_cmd,
        # Start the ros2 bag record process
        OpaqueFunction(function=launch_setup),
    ])


if __name__ == '__main__':
    generate_launch_description()
